"""
* 名称：CobotGPT_V5_APF轨迹优化.py
* 功能：搭载APF算法，并实现碰撞检测和轨迹优化
* 缺陷：APF算法运行速度缓慢(13s-260s)
"""

import queue
import time
import timeit

import numpy as np
import torch
import torch.nn as nn

from Modules.APF.Modules.robot_class import Joint_point, Cube_obstacle
from Modules.APF.User.Releases.Artifial_potential_field_RRT import APF_RRT, get_obstacle
from Modules.AUBO.User.Releases.robotcontrol import *
from Modules.KINECT.User.Releases.kinectcontrol import *
from Modules.robot_class import *


class DNN(nn.Module):  # 继承自PyTorch的nn.Module类
    """
    * 功能：碰撞检测模型
    """

    def __init__(self):
        """ 搭建神经网络各层 """
        # 调用父类的初始化方法,这是在子类中经常使用的技巧，以确保父类的初始化方法被正确调用
        super(DNN, self).__init__()
        self.net = nn.Sequential(  # 按顺序搭建各层
            nn.Linear(36, 32), nn.Sigmoid(),  # 第 1 层：全连接层
            nn.Linear(32, 8), nn.Sigmoid(),  # 第 2 层：全连接层
            nn.Linear(8, 4), nn.Sigmoid(),  # 第 3 层：全连接层
            nn.Linear(4, 1), nn.Sigmoid()  # 第 4 层：全连接层
        )

    def forward(self, x):
        """ 前向传播 """
        y = self.net(x)  # x 即输入数据
        return y  # y 即输出数据


def delay(microseconds):
    """
    功能：延迟xx微秒，1s = 1*10^3 ms = 1*10^6 us = 1*10^9 ns
    输入：
    输出
    """
    start = timeit.default_timer()
    while True:
        elapsed = (timeit.default_timer() - start) * 10 ** 6
        if elapsed >= microseconds:
            break
    return 0


def Link_Transformation(last_i, i, a_list, alpha_list, d_list, theta_list):
    """
    * function：坐标系{i-1}到坐标系{i}的转换矩阵
    * tips：这里的last_i指的是i-1
    """
    i = i  # 下面使用的i-1表示列表的第i-1个数，注意同DH参数里的i-1区别
    T_martix = np.mat(np.zeros((4, 4)))

    T_martix[0, 0] = np.cos(theta_list[i - 1])
    T_martix[0, 1] = -1 * np.sin(theta_list[i - 1])
    T_martix[0, 2] = 0
    T_martix[0, 3] = a_list[i - 1]

    T_martix[1, 0] = np.sin(theta_list[i - 1]) * np.cos(alpha_list[i - 1])
    T_martix[1, 1] = np.cos(theta_list[i - 1]) * np.cos(alpha_list[i - 1])
    T_martix[1, 2] = -1 * np.sin(alpha_list[i - 1])
    T_martix[1, 3] = -1 * np.sin(alpha_list[i - 1]) * d_list[i - 1]

    T_martix[2, 0] = np.sin(theta_list[i - 1]) * np.sin(alpha_list[i - 1])
    T_martix[2, 1] = np.cos(theta_list[i - 1]) * np.sin(alpha_list[i - 1])
    T_martix[2, 2] = np.cos(alpha_list[i - 1])
    T_martix[2, 3] = np.cos(alpha_list[i - 1]) * d_list[i - 1]

    T_martix[3, 0] = 0
    T_martix[3, 1] = 0
    T_martix[3, 2] = 0
    T_martix[3, 3] = 1

    return T_martix


def get_robot_position(joint_radian: tuple = (0, 0, 0, 0, 0, 0)):
    """
    * 功能：机器人正解，获取六个关节坐标
    * 输入：机械臂关节角
    * 输出：六个关节坐标：6*3=18，类型tuple，单位m
    """
    # 初始化参数（DH参数）
    a_list = [0, 0, 408, 376, 0, 0]
    alpha_list = [0, np.pi / 2, np.pi, np.pi, np.pi / 2, np.pi / 2]
    d_list = [98.5, 121.5, 0, 0, 102.5, 94]
    # theta_list = [0, np.pi / 2, 0, np.pi / 2, np.pi, 0]  # 输入想要转动的角度（此处设置转动的角度在转动后即达到机械臂伸直状态）
    theta_offset = [0, np.pi / 2, 0, np.pi / 2, np.pi, 0]  # 关节角补偿
    # print(theta_offset)
    joint_radian = list(joint_radian)
    # print(joint_radian)
    theta_list = [a + b for a, b in zip(joint_radian, theta_offset)]  # 机械臂关节角：正转+，反转-
    # print(theta_list)

    # 例：T_0_1表示坐标系{0}到坐标系{1}的转换矩阵
    T_0_1 = Link_Transformation(0, 1, a_list, alpha_list, d_list, theta_list)
    T_1_2 = Link_Transformation(1, 2, a_list, alpha_list, d_list, theta_list)
    T_2_3 = Link_Transformation(2, 3, a_list, alpha_list, d_list, theta_list)
    T_3_4 = Link_Transformation(3, 4, a_list, alpha_list, d_list, theta_list)
    T_4_5 = Link_Transformation(4, 5, a_list, alpha_list, d_list, theta_list)
    T_5_6 = Link_Transformation(5, 6, a_list, alpha_list, d_list, theta_list)

    T_0_6 = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5 * T_5_6  # 二维数组，形状(4,4)，即矩阵
    T_0_5 = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5
    T_0_4 = T_0_1 * T_1_2 * T_2_3 * T_3_4
    T_0_3 = T_0_1 * T_1_2 * T_2_3
    T_0_2 = T_0_1 * T_1_2
    T_0_1 = T_0_1
    # print(type(T_0_6))    # 查看数组形状：(4,4)
    # T_0_6 = np.array(T_0_6)
    # print(type(T_0_6))

    # 机器人关节坐标
    joint1_pos = T_0_1[0:3, 3].copy()
    joint2_pos = T_0_2[0:3, 3].copy()
    joint3_pos = T_0_3[0:3, 3].copy()
    joint4_pos = T_0_4[0:3, 3].copy()
    joint5_pos = T_0_5[0:3, 3].copy()
    joint6_pos = T_0_6[0:3, 3].copy()
    # print("joint1_pos: ",joint1_pos)
    # print("joint2_pos: ",joint2_pos)
    # print("joint3_pos: ",joint3_pos)
    # print("joint4_pos: ",joint4_pos)
    # print("joint5_pos: ",joint5_pos)
    # print("joint6_pos: ",joint6_pos)

    # <class 'numpy.matrix'> 转换为 <class 'numpy.ndarray'>
    arr1 = np.array(joint1_pos)
    arr2 = np.array(joint2_pos)
    arr3 = np.array(joint3_pos)
    arr4 = np.array(joint4_pos)
    arr5 = np.array(joint5_pos)
    arr6 = np.array(joint6_pos)
    # print(type(arr1))
    # print(arr6)
    # arr1 = arr1.astype(float)
    # arr2 = arr2.astype(float)
    # arr3 = arr3.astype(float)
    # arr4 = arr4.astype(float)
    # arr5 = arr5.astype(float)
    # arr6 = arr6.astype(float)

    # 使用flatten()方法将矩阵降级为一维数组
    flat_array_1 = arr1.flatten() / 1000.0
    flat_array_2 = arr2.flatten() / 1000.0
    flat_array_3 = arr3.flatten() / 1000.0
    flat_array_4 = arr4.flatten() / 1000.0
    flat_array_5 = arr5.flatten() / 1000.0
    flat_array_6 = arr6.flatten() / 1000.0
    # print(flat_array_1)
    # print(flat_array_2)
    # print(flat_array_3)
    # print(flat_array_4)
    # print(flat_array_5)
    # print(flat_array_6)

    # 使用tuple()函数将一维数组转换为元组
    # 改为list
    tuple1_arr = list(flat_array_1)
    tuple2_arr = list(flat_array_2)
    tuple3_arr = list(flat_array_3)
    tuple4_arr = list(flat_array_4)
    tuple5_arr = list(flat_array_5)
    tuple6_arr = list(flat_array_6)
    # print(tuple1_arr)
    # print(tuple2_arr)
    # print(tuple3_arr)
    # print(tuple4_arr)
    # print(tuple5_arr)
    # print(tuple6_arr)
    tuple_all = tuple1_arr + tuple2_arr + tuple3_arr + tuple4_arr + tuple5_arr + tuple6_arr
    # print(tuple_all)
    # print(type(tuple_all))

    joint_position = tuple_all
    # print(T_0_6)
    return joint_position


class CobotGPT:

    def __init__(self):
        """
        功能：设定类里面通用的一些属性，静态值，不能放动态的
        输入：
        输出：连接状态码
        """

        # 配置服务器地址和端口
        # self.ip = '169.254.94.55'
        self.ip = '192.168.1.6'  # 一楼：注意配置网线口的适配器属性：关掉IPV6，手动设置IPV4地址和机械臂在同一个网段（README）
        self.port = 8899

        # 预设的机械臂工作路径点：9个
        self.joint_radian_list =[
                    (10 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (20 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (30 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (40 * pi / 180, 0 * pi / 180, 10 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (40 * pi / 180, 0 * pi / 180, 20 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (40 * pi / 180, 0 * pi / 180, 30 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (40 * pi / 180, 0 * pi / 180, 40 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (40 * pi / 180, 0 * pi / 180, 50 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (40 * pi / 180, 0 * pi / 180, 60 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (40 * pi / 180, 0 * pi / 180, 70 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (50 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (60 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (70 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (80 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (90 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (100 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (110 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (120 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (130 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (140 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (150 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (160 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),  # 1
                    (160 * pi / 180, -10 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (160 * pi / 180, -20 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),  # 第一阶段
                    (150 * pi / 180, -20 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (140 * pi / 180, -10 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (130 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (120 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (110 * pi / 180, -10 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (100 * pi / 180, -20 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (90 * pi / 180, -10 * pi / 180, 80 * pi / 180, 0 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (80 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (70 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (60 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (50 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 90 * pi / 180, 0 * pi / 180),
                    (30 * pi / 180, 0 * pi / 180, 40 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                    (10 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180)  # 复位
                    ]
        # self.joint_radian_list = [
        #     (10 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (20 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (30 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (40 * pi / 180, 0 * pi / 180, 10 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (40 * pi / 180, 0 * pi / 180, 20 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (40 * pi / 180, 0 * pi / 180, 30 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (40 * pi / 180, 0 * pi / 180, 40 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (40 * pi / 180, 0 * pi / 180, 50 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (40 * pi / 180, 0 * pi / 180, 60 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (40 * pi / 180, 0 * pi / 180, 70 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (50 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (60 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (70 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (80 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (90 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (100 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (110 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (120 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (130 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (140 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (150 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (160 * pi / 180, 0 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),  # 1
        #     (160 * pi / 180, -10 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (160 * pi / 180, -20 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 0 * pi / 180, 0 * pi / 180),
        #     (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),  # 第一阶段(goal)
        #
        #     (160 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
        #     (90 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
        #     (90 * pi / 180, -20 * pi / 180, 80 * pi / 180, 10 * pi / 180, 90 * pi / 180, 0 * pi / 180),
        #     (0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180)]  # 复位

        # 轨迹优化起点，终点
        self.start = None
        self.goal = None
        self.robot_step = int()

    def robot_set_position(self, joint_radian_queue: Queue, robot_step_queue: Queue):
        """
        * 功能：设置机械臂的固定轨迹点，并实时获取关节角信息
        * 输入：joint_radian_queue机械臂关节角(multiprocessing.Queue，maxsize=1)
        *       robot_step_queue机器人轨迹步数(multiprocessing.Queue，maxsize=1)
        * 输出：
        """

        # 初始化logger
        logger_init()
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("Func1: robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                logger.info("{0} robot_set_position() running...".format(Auboi5Robot.get_local_time()))
                # 上电
                robot.robot_startup()
                # 设置碰撞等级
                robot.set_collision_class(10)
                # 初始化全局配置文件
                robot.init_profile()
                # robot.enable_robot_event()

                # 设置关节最大加速度
                robot.set_joint_maxacc((1.5, 1.5, 1.5, 1.5, 1.5, 1.5))
                # 设置关节最大速度
                # robot.set_joint_maxvelc((0.2, 0.2, 0.2, 0.2, 0.2, 0.2))
                # robot.set_joint_maxvelc((0.1, 0.1, 0.1, 0.1, 0.1, 0.1))  # 低速挡
                # robot.set_joint_maxvelc((0.3, 0.3, 0.3, 0.3, 0.3, 0.3))  # 高速挡
                robot.set_joint_maxvelc((0.5, 0.5, 0.5, 0.5, 0.5, 0.5))  # 高速挡

                # 回到初始位姿(快速复位)
                joint_radian_ori = (0, 0, 0, 0, 0, 0)  # 初始位姿,turple类型
                logger.info("回到初始位姿：{0}".format(joint_radian_ori))
                robot.move_joint(joint_radian_ori)
                robot.set_joint_maxvelc((0.2, 0.2, 0.2, 0.2, 0.2, 0.2))  # 中速档
                # robot.set_joint_maxvelc((0.3, 0.3, 0.3, 0.3, 0.3, 0.3))  # 高速挡
                # robot.set_joint_maxvelc((0.1, 0.1, 0.1, 0.1, 0.1, 0.1))  # 低速挡
                logger.info("机器人开始工作......")

                while result == RobotErrorType.RobotError_SUCC:
                    if joint_radian_queue.full() and robot_step_queue.full() is True:
                        try:
                            joint_radian = joint_radian_queue.get(timeout = 0.1)  # 从管道获取下一关节角
                            self.robot_step = robot_step_queue.get(timeout = 0.1)  # 从管道获取相应的机器人步数(多进程同步)
                            logger.info("[step {1}]: move joint to {0}".format(joint_radian, self.robot_step))
                            robot.move_joint(joint_radian)
                        except queue.Empty:
                            logger.info("TimeOut(Func1): joint_radian_queue or robot_step_queue is empty")

        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in robot_set_position()".format(robot.get_local_time(), e))

        except KeyboardInterrupt:
            robot.move_stop()
            # 断开服务器链接
            if robot.connected:
                # 断开机械臂链接
                robot.disconnect()
            # 释放库资源
            Auboi5Robot.uninitialize()
            logger.info("KeyboardInterrupt: robot_set_position()")

        # 无论是否有异常，都会执行的代码
        finally:
            logger.info("{0} robot_set_position() is disconnected...".format(Auboi5Robot.get_local_time()))

    def dataset(self, skeleton_position_queue: Queue, position_data_queue: Queue):
        """
        功能：将坐标数据转换成数据集
        输入：skeleton_position_queue:输入骨骼坐标，position_data_queue：输出36列一维数据
        输出：
        """

        # 初始化logger
        logger_init()
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("Func2: robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                logger.info("Func2：{0} dataset() running...".format(Auboi5Robot.get_local_time()))
                # # 上电
                # robot.robot_startup()
                # # 设置碰撞等级
                # robot.set_collision_class(10)
                # # 初始化全局配置文件
                # robot.init_profile()

                while result == RobotErrorType.RobotError_SUCC:
                    # 获取当前机械臂六个关节角
                    current_waypoint = robot.get_current_waypoint()
                    data1 = current_waypoint["joint"]

                    # 将此刻skeleton_position_queue管道（骨骼坐标）清空，并获取最新骨骼坐标
                    while skeleton_position_queue.empty() is not True:
                        try:
                            skeleton_position_queue.get(timeout=0.1)
                        except queue.Empty:
                            pass

                    try:
                        data2 = skeleton_position_queue.get(timeout = 0.1)
                    except queue.Empty:
                        data2 = [0.0]
                        # logger.info("TimeOut: skeleton_position_queue is empty")

                    # if skeleton_position_queue.full() is True:
                    #     try:
                    #         data2 = skeleton_position_queue.get(timeout = 0.1)
                    #     except queue.Empty:
                    #         logger.info("TimeOut: skeleton_position_queue is empty")
                    # else:
                    #     data2 = [0.0]

                    # position_data_queue管道（36列一维数据）放入数据（36列：骨骼坐标 + 六个关节角）
                    data = data1 + data2

                    # position_data_queue管道（36列一维数据）清空，
                    while position_data_queue.empty() is not True:
                        try:
                            position_data_queue.get(timeout=0.1)
                        except queue.Empty:
                            pass
                            # logger.info("TimeOut: position_data_queue is empty")

                    # if position_data_queue.empty() is True:
                    try:
                        position_data_queue.put(data, timeout = 0.1)
                        # print(f"position_data: {data}")
                    except queue.Full:
                        pass
                        # logger.info("TimeOut: position_data_queue is already fully")
                logger.info("==========dataset fail===========")

        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in dataset()".format(robot.get_local_time(), e))

        except KeyboardInterrupt:
            robot.move_stop()
            # 断开服务器链接
            if robot.connected:
                # 断开机械臂链接
                robot.disconnect()
            # 释放库资源
            Auboi5Robot.uninitialize()
            logger.info("KeyboardInterrupt: dataset()")

        # 无论是否有异常，都会执行的代码
        finally:
            logger.info("{0} dataset() is disconnected...".format(Auboi5Robot.get_local_time()))

    def collision_detection(self, next_joint_radian_queue: Queue, position_data_queue: Queue, robot_step_queue: Queue):
        """
        功能：碰撞检测
        输入：robot_step_queue:机器人轨迹步数，
        next_joint_radian_queue:输出下一个关节角，
        position_data_queue：输入36列一维数据
        输出：
        """

        # 初始化logger
        logger_init()
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("Func3：robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                logger.info("{0} collision_detection() running...".format(Auboi5Robot.get_local_time()))
                # # 上电
                # robot.robot_startup()
                # # 设置碰撞等级
                # robot.set_collision_class(10)
                # # 初始化全局配置文件
                # robot.init_profile()

                # 加载离线碰撞检测模型(DNN)
                model_collision_detection = torch.load('model_collision_detection_v1.pth')  # 加载训练好的碰撞检测模型
                model_collision_detection.to('cuda:0')  # 模型加载到GPU上

                # 设定机器人的计算关节角空间
                joint_limit_angle = [
                    Joint_point(
                        [-170 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180,
                         -170 * pi / 180]),
                    Joint_point([170 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180,
                                 170 * pi / 180])]

                # 初始化机器人步数和碰撞标志
                self.robot_step = 0
                pred = 0
                cnt = 1

                # model_path_optimize = torch.load('DNN_model2.pth')  # 加载训练好的轨迹优化模型
                # model_path_optimize.to('cuda:0')  # 模型加载到GPU上

                while result == RobotErrorType.RobotError_SUCC:
                    # next_joint_radian_queue（下一个关节角）管道战术清空
                    # while next_joint_radian_queue.empty() is not True:
                    #     next_joint_radian_queue.get()

                    # # position_data_queue（输入36列一维数据）管道战术清空
                    # while position_data_queue.empty() is not True:
                    #     position_data_queue.get()

                    # 运行正常，传输下一个关节角给机械臂
                    # if collision_flag == 0:
                    #     # print("collision 运行正常传输关节角")
                    #     # collision_flag_queue.put(0)
                    #     next_joint_radian_queue.put(self.joint_radian_list[step])
                    #     step += 1
                    #     # 走完一轮，将步数清空，重头开始轨迹运动
                    #     if step == int(len(self.joint_radian_list)):
                    #         step = 0
                    #         # print("step reset!!!")

                    # 碰撞检测模型输入数据获取, 36列
                    if position_data_queue.full() is True:
                        try:
                            X = position_data_queue.get(timeout = 0.1)  # 1s的等待时间
                        except queue.Empty:
                            X = [0.0]
                            # logger.info("TimeOut: position_data_queue is empty")
                    else:
                        X = [0.0]

                    # # 轨迹优化：模型输入数据获取，
                    # # 组成：机器人关节坐标(6*3=18)，人体骨骼坐标(10*3=30)，当前关节角(6)，目标点关节角(6)
                    # joint_radian = X[0:6]  # 机器人关节角，6列
                    # skeleton_data = X[5:-1]  # 人体骨骼坐标，30列
                    # robot_position = get_robot_position(joint_radian)  # 机器人关节坐标，18列
                    # # O = robot_position + skeleton_data  # 获取48列一维数据，轨迹优化

                    # print(f"self.data:{self.data}")
                    # print(len(X))
                    # print(X)

                    # 碰撞检测模块，用模型1
                    if len(X) == 36:
                        if cnt:
                            time.sleep(2)
                            logger.info("前2s骨骼数据先不要")
                            cnt -= 1
                        else:
                            # 轨迹优化：模型输入数据获取，
                            # 组成：机器人关节坐标(6*3=18)，人体骨骼坐标(10*3=30)，当前关节角(6)，目标点关节角(6)
                            # joint_radian = X[0:6]  # 机器人关节角，6列
                            # skeleton_data = X[5:-1]  # 人体骨骼坐标，30列
                            # robot_position = get_robot_position(joint_radian)  # 机器人关节坐标，18列

                            arr = np.array(X)  # 列表 => numpy数组
                            arr = arr.astype(np.float32)  # 转为 float32 类型numpy数组
                            ts = torch.tensor(arr)  # numpy数组 => 张量
                            ts = ts.to('cuda')
                            with torch.no_grad():  # 该局部关闭梯度计算功能
                                pred = model_collision_detection(ts)  # 一次前向传播（批量）
                                pred[pred >= 0.5] = 1
                                pred[pred < 0.5] = 0
                                pred = int(pred.item())  # 张量 => int值
                                # print(f"是否碰撞: {pred}")
                                logger.info("是否碰撞: {0}".format(pred))
                    else:
                        # print("输入的总坐标格式错误")
                        pred = 0

                    # 碰撞标志处理
                    if pred == 0:
                        if next_joint_radian_queue.empty() and robot_step_queue.empty() is True:
                            try:
                                # logger.info("robot_step {0}".format(robot_step))
                                next_joint_radian_queue.put(self.joint_radian_list[self.robot_step], timeout = 0.1)
                                robot_step_queue.put(self.robot_step, timeout = 0.1)
                                self.robot_step += 1
                                if self.robot_step == int(len(self.joint_radian_list)):
                                    self.robot_step = 0
                            except queue.Full:
                                pass
                                # logger.info("TimeOut: next_joint_radian_queue or robot_step_queue is already fully")

                    # 碰撞检测结果处理模块
                    if pred == 1:
                        # 机器人暂停运动
                        robot.move_pause()
                        logger.info("robot pause=============")

                        # 获取的当前的关节坐标、骨骼坐标信息、当前关节角、目标关节角

                        # collision_skeleton_position = list(robot.forward_kin(tuple(joint_radian)))  # 把机械臂末端当作障碍物
                        # skeleton_position_list = list(collision_skeleton_position)  # 3*10=30，骨骼坐标
                        # skeleton_position_list = list(skeleton_data)  # 3*10=30，骨骼坐标

                        # 获取当前机械臂六个关节角（机械臂停止的时候）
                        current_waypoint = robot.get_current_waypoint()
                        joint_radian = current_waypoint["joint"]
                        robot_position = get_robot_position(tuple(joint_radian))  # 机器人关节坐标，18列
                        # print(f"robot_position: {robot_position}")
                        joint_position_list = list(robot_position)  # 3*6=18，机器人6个关节坐标


                        step_reset = self.robot_step - 2    # 后退一步作为起点（上边已经+1了）
                        if step_reset < 0:
                            step_reset = 0
                        start = list(self.joint_radian_list[step_reset])  # 上一步作为起点(关节角)
                        goal = [160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                0 * pi / 180]  # 目标关节角(goal)
                        self.start = Joint_point(start)
                        self.goal = Joint_point(goal)

                        # print(f"joint_position_list[14:-1]: {joint_position_list[14:-1]}")
                        # 先确定障碍物坐标，即找出距离机器人关节最近的骨骼点坐标，并将骨骼点坐标实例化为实体障碍物
                        # cube_obstacle = get_obstacle(joint_position_list[15:-1], joint_position_list)
                        # cube_obstacle.x -= 0.2

                        l = robot.forward_kin(tuple(joint_radian))
                        cube_obstacle = Cube_obstacle(Point(joint_position_list[15:]), 0.2, 0.2, 0.2)
                        print("obstacle positiopn(xyz,lwh):", cube_obstacle.x, cube_obstacle.y, cube_obstacle.z,
                              cube_obstacle.l, cube_obstacle.w, cube_obstacle.h)
                        # print("robot bottom positiopn（正解）",l['pos'])
                        print("current joint radian:", joint_radian)
                        print("self.start:", self.start.theta)
                        print("self.goal:", self.goal.theta)

                        # 轨迹优化：运行人工势场法
                        apf_rrt = APF_RRT(self.start, self.goal, cube_obstacle, joint_limit_angle)

                        start = time.time()
                        path = apf_rrt.path_planning()
                        end = time.time()
                        print("轨迹优化完成, 耗费{:.2f}s".format(end - start))

                        # 运行轨迹优化路径点
                        path_list = list()
                        for i in range(len(path)):
                            path_list.append(path[i].theta)
                        # print("path_list:", path_list)

                        # 机器人恢复运动
                        robot.move_continue()
                        logger.info("robot continue=====")
                        self.robot_step = -999  # 表示此时是轨迹优化的步数

                        # next_joint_radian_queue（下一个关节角）管道战术清空
                        while next_joint_radian_queue.empty() is not True:
                            try:
                                next_joint_radian_queue.get(timeout=0.1)
                            except queue.Empty:
                                pass
                        # robot_step_queue（轨迹步数）管道战术清空
                        while robot_step_queue.empty() is not True:
                            try:
                                robot_step_queue.get(timeout=0.1)
                            except queue.Empty:
                                pass
                        # 后退一步
                        # if next_joint_radian_queue.empty() and robot_step_queue.empty() is True:
                        #     try:
                        #         # logger.info("robot_step {0}".format(robot_step))
                        #         next_joint_radian_queue.put(self.joint_radian_list[step_reset], timeout = 0.1)
                        #         robot_step_queue.put(self.robot_step, timeout = 0.1)
                        #     except queue.Full:
                        #         pass
                                # logger.info("TimeOut: next_joint_radian_queue or robot_step_queue is already fully")

                        for i in range(len(path_list)):
                            path = tuple(path_list[i])

                            try:
                                next_joint_radian_queue.put(path, timeout = 100)
                                robot_step_queue.put(self.robot_step, timeout = 100)
                                logger.info("[轨迹优化路径点 {1}, step {2}]: {0}".format(path, i, self.robot_step))
                            except queue.Full:
                                # pass
                                logger.info("TimeOut(Func3): next_joint_radian_queue or robot_step_queue is already fully")

                        self.robot_step = 0  # 表示机器人恢复正常轨迹运动
                logger.info("==========collision_detection fail===========")

        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in collision_detection()".format(robot.get_local_time(), e))

        except KeyboardInterrupt:
            robot.move_stop()
            # 断开服务器链接
            if robot.connected:
                # 断开机械臂链接
                robot.disconnect()
            # 释放库资源
            Auboi5Robot.uninitialize()
            logger.info("KeyboardInterrupt: collision_detection()")

        # 无论是否有异常，都会执行的代码
        finally:
            logger.info("{0} collision_detection() is disconnected...".format(Auboi5Robot.get_local_time()))


if __name__ == '__main__':
    # 创建类实例
    game = BodyGameRuntime()
    cobot = CobotGPT()

    # 创建通信队列
    q1 = Queue(maxsize = 1)  # joint_radian_queue, next_joint_radian_queue
    q2 = Queue(maxsize = 1)  # robot_step_queue
    q3 = Queue(maxsize = 1)  # skeleton_position_queue
    q4 = Queue(maxsize = 1)  # position_data_queue

    # 创建子进程
    p1 = Process(target = cobot.robot_set_position,
                 kwargs = {"joint_radian_queue": q1, "robot_step_queue": q2})
    p2 = Process(target = cobot.dataset,
                 kwargs = {"skeleton_position_queue": q3, "position_data_queue": q4})
    p3 = Process(target = cobot.collision_detection,
                 kwargs = {"next_joint_radian_queue": q1, "position_data_queue": q4, "robot_step_queue": q2})
    p1.start()
    p2.start()
    p3.start()

    # 主进程
    game.run(q3)
